﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.IO.Ports;
using System.Windows.Forms;
using WFNetLib;

namespace RobotControl
{
    public class 推进器
    {  
        public static int[] k,b,nk,y0,y5,yn5;
        SerialPort com;
        static public int Power=80;
        public void Test(int i,bool bN)
        {
            if (OpenPort())
            {
                Union_UInt16 p1 = new Union_UInt16();
                if (bN)
                {
                    p1.ofs_16 = (ushort)(nk[i] * (Power * -1) * 5 / 100 + b[i]);
                }
                else
                {
                    p1.ofs_16 = (ushort)(k[i] * (Power) * 5 / 100 + b[i]);
                }
                byte[] tx = new byte[6];
                tx[0] = 0xaa;
                tx[1] = (byte)(i+1);//通道号
                tx[2] = BytesOP.GetLowNibble(p1.ofs1_8);
                tx[3] = BytesOP.GetHighNibble(p1.ofs0_8);
                tx[4] = BytesOP.GetLowNibble(p1.ofs0_8);
                tx[5] = 0xed;
                com.Write(tx, 0, 6);                
                com.Close();
            }
        }
        public void GoHead()
        {
            if (OpenPort())
            {
                Union_UInt16 p1 = new Union_UInt16();
                p1.ofs_16 = (ushort)(nk[3] * (Power * -1) * 5 / 100 + b[3]);
                Union_UInt16 p2 = new Union_UInt16();
                p2.ofs_16 = (ushort)(nk[0] * (Power * -1) * 5 / 100 + b[0]);
                byte[] tx = new byte[6];
                tx[0] = 0xaa;
                tx[1] = 0x04;//通道号
                tx[2] = BytesOP.GetLowNibble(p1.ofs1_8);
                tx[3] = BytesOP.GetHighNibble(p1.ofs0_8);
                tx[4] = BytesOP.GetLowNibble(p1.ofs0_8);
                tx[5] = 0xed;
                com.Write(tx, 0, 6);
                tx[0] = 0xaa;
                tx[1] = 0x01;//通道号
                tx[2] = BytesOP.GetLowNibble(p2.ofs1_8);
                tx[3] = BytesOP.GetHighNibble(p2.ofs0_8);
                tx[4] = BytesOP.GetLowNibble(p2.ofs0_8);
                tx[5] = 0xed;
                com.Write(tx, 0, 6);
                com.Close();
            }            
        }
        public void GoBack()
        {
            if (OpenPort())
            {
                Union_UInt16 p1 = new Union_UInt16();
                p1.ofs_16 = (ushort)(k[3] * Power * 5 / 100 + b[3]);
                Union_UInt16 p2 = new Union_UInt16();
                p2.ofs_16 = (ushort)(k[0] * Power * 5 / 100 + b[0]);
                byte[] tx = new byte[6];
                tx[0] = 0xaa;
                tx[1] = 0x04;//通道号
                tx[2] = BytesOP.GetLowNibble(p1.ofs1_8);
                tx[3] = BytesOP.GetHighNibble(p1.ofs0_8);
                tx[4] = BytesOP.GetLowNibble(p1.ofs0_8);
                tx[5] = 0xed;
                com.Write(tx, 0, 6);
                tx[0] = 0xaa;
                tx[1] = 0x01;//通道号
                tx[2] = BytesOP.GetLowNibble(p2.ofs1_8);
                tx[3] = BytesOP.GetHighNibble(p2.ofs0_8);
                tx[4] = BytesOP.GetLowNibble(p2.ofs0_8);
                tx[5] = 0xed;
                com.Write(tx, 0, 6);
                com.Close();
            }
        }
        public void GoLeft()
        {
            if (OpenPort())
            {
                Union_UInt16 p1 = new Union_UInt16();
                p1.ofs_16 = (ushort)(k[2] * Power * 5 / 100 + b[2]);
                Union_UInt16 p2 = new Union_UInt16();
                p2.ofs_16 = (ushort)(k[4] * Power * 5 / 100 + b[4]);
                byte[] tx = new byte[6];
                tx[0] = 0xaa;
                tx[1] = 0x03;//通道号
                tx[2] = BytesOP.GetLowNibble(p1.ofs1_8);
                tx[3] = BytesOP.GetHighNibble(p1.ofs0_8);
                tx[4] = BytesOP.GetLowNibble(p1.ofs0_8);
                tx[5] = 0xed;
                com.Write(tx, 0, 6);
                tx[0] = 0xaa;
                tx[1] = 0x05;//通道号
                tx[2] = BytesOP.GetLowNibble(p2.ofs1_8);
                tx[3] = BytesOP.GetHighNibble(p2.ofs0_8);
                tx[4] = BytesOP.GetLowNibble(p2.ofs0_8);
                tx[5] = 0xed;
                com.Write(tx, 0, 6);
                com.Close();
            }
        }
        public void GoRight()
        {
            if (OpenPort())
            {
                Union_UInt16 p1 = new Union_UInt16();
                p1.ofs_16 = (ushort)(k[2] * (Power*-1) * 5 / 100 + b[2]);
                Union_UInt16 p2 = new Union_UInt16();
                p2.ofs_16 = (ushort)(k[4] * (Power*-1) * 5 / 100 + b[4]);
                byte[] tx = new byte[6];
                tx[0] = 0xaa;
                tx[1] = 0x03;//通道号
                tx[2] = BytesOP.GetLowNibble(p1.ofs1_8);
                tx[3] = BytesOP.GetHighNibble(p1.ofs0_8);
                tx[4] = BytesOP.GetLowNibble(p1.ofs0_8);
                tx[5] = 0xed;
                com.Write(tx, 0, 6);
                tx[0] = 0xaa;
                tx[1] = 0x05;//通道号
                tx[2] = BytesOP.GetLowNibble(p2.ofs1_8);
                tx[3] = BytesOP.GetHighNibble(p2.ofs0_8);
                tx[4] = BytesOP.GetLowNibble(p2.ofs0_8);
                tx[5] = 0xed;
                com.Write(tx, 0, 6);
                com.Close();
            }
        }
        public void GoTop()
        {
            if (OpenPort())
            {
                Union_UInt16 p1 = new Union_UInt16();
                p1.ofs_16 = (ushort)(k[0] * Power * 5 / 100 + b[0]);
                Union_UInt16 p2 = new Union_UInt16();
                p2.ofs_16 = (ushort)(k[1] * Power * 5 / 100 + b[1]);
                byte[] tx = new byte[6];
                tx[0] = 0xaa;
                tx[1] = 0x01;//通道号
                tx[2] = BytesOP.GetLowNibble(p1.ofs1_8);
                tx[3] = BytesOP.GetHighNibble(p1.ofs0_8);
                tx[4] = BytesOP.GetLowNibble(p1.ofs0_8);
                tx[5] = 0xed;
                com.Write(tx, 0, 6);
                tx[0] = 0xaa;
                tx[1] = 0x02;//通道号
                tx[2] = BytesOP.GetLowNibble(p2.ofs1_8);
                tx[3] = BytesOP.GetHighNibble(p2.ofs0_8);
                tx[4] = BytesOP.GetLowNibble(p2.ofs0_8);
                tx[5] = 0xed;
                com.Write(tx, 0, 6);
                com.Close();
            }
        }
        public void GoBottom()
        {
            if (OpenPort())
            {
                Union_UInt16 p1 = new Union_UInt16();
                p1.ofs_16 = (ushort)(k[0] * Power * 5 / 100 + b[0]);
                Union_UInt16 p2 = new Union_UInt16();
                p2.ofs_16 = (ushort)(k[1] * Power * 5 / 100 + b[1]);
                byte[] tx = new byte[6];
                tx[0] = 0xaa;
                tx[1] = 0x01;//通道号
                tx[2] = BytesOP.GetLowNibble(p1.ofs1_8);
                tx[3] = BytesOP.GetHighNibble(p1.ofs0_8);
                tx[4] = BytesOP.GetLowNibble(p1.ofs0_8);
                tx[5] = 0xed;
                com.Write(tx, 0, 6);
                tx[0] = 0xaa;
                tx[1] = 0x02;//通道号
                tx[2] = BytesOP.GetLowNibble(p2.ofs1_8);
                tx[3] = BytesOP.GetHighNibble(p2.ofs0_8);
                tx[4] = BytesOP.GetLowNibble(p2.ofs0_8);
                tx[5] = 0xed;
                com.Write(tx, 0, 6);
                com.Close();
            }
        }
        public void GoLeftRound()
        {
            if (OpenPort())
            {
                Union_UInt16 p1 = new Union_UInt16();
                p1.ofs_16 = (ushort)(k[2] * Power * 5 / 100 + b[2]);
                Union_UInt16 p2 = new Union_UInt16();
                p2.ofs_16 = (ushort)(k[4] * (Power*-1) * 5 / 100 + b[4]);
                byte[] tx = new byte[6];
                tx[0] = 0xaa;
                tx[1] = 0x03;//通道号
                tx[2] = BytesOP.GetLowNibble(p1.ofs1_8);
                tx[3] = BytesOP.GetHighNibble(p1.ofs0_8);
                tx[4] = BytesOP.GetLowNibble(p1.ofs0_8);
                tx[5] = 0xed;
                com.Write(tx, 0, 6);
                tx[0] = 0xaa;
                tx[1] = 0x05;//通道号
                tx[2] = BytesOP.GetLowNibble(p2.ofs1_8);
                tx[3] = BytesOP.GetHighNibble(p2.ofs0_8);
                tx[4] = BytesOP.GetLowNibble(p2.ofs0_8);
                tx[5] = 0xed;
                com.Write(tx, 0, 6);
                com.Close();
            }
        }
        public void GoRightRound()
        {
            if (OpenPort())
            {
                Union_UInt16 p1 = new Union_UInt16();
                p1.ofs_16 = (ushort)(k[2] * (Power*-1) * 5 / 100 + b[2]);
                Union_UInt16 p2 = new Union_UInt16();
                p2.ofs_16 = (ushort)(k[4] * Power * 5 / 100 + b[4]);
                byte[] tx = new byte[6];
                tx[0] = 0xaa;
                tx[1] = 0x03;//通道号
                tx[2] = BytesOP.GetLowNibble(p1.ofs1_8);
                tx[3] = BytesOP.GetHighNibble(p1.ofs0_8);
                tx[4] = BytesOP.GetLowNibble(p1.ofs0_8);
                tx[5] = 0xed;
                com.Write(tx, 0, 6);
                tx[0] = 0xaa;
                tx[1] = 0x05;//通道号
                tx[2] = BytesOP.GetLowNibble(p2.ofs1_8);
                tx[3] = BytesOP.GetHighNibble(p2.ofs0_8);
                tx[4] = BytesOP.GetLowNibble(p2.ofs0_8);
                tx[5] = 0xed;
                com.Write(tx, 0, 6);
                com.Close();
            }
        }
        public void Stop(int x)
        {
            if (OpenPort())
            {                
                byte[] tx = new byte[6];
                Union_UInt16 p = new Union_UInt16();
                if(x==-1)
                {                    
                    for (int i = 0; i < 6; i++)
                    {
                        p.ofs_16 = (ushort)(b[i]);
                        tx[0] = 0xaa;
                        tx[1] = (byte)(i+1);//通道号
                        tx[2] = BytesOP.GetLowNibble(p.ofs1_8);
                        tx[3] = BytesOP.GetHighNibble(p.ofs0_8);
                        tx[4] = BytesOP.GetLowNibble(p.ofs0_8);
                        tx[5] = 0xed;
                        com.Write(tx, 0, 6);
                    }
                }
                else if (x == 1)//停止左右
                {
                    p.ofs_16 = (ushort)(b[2]);
                    tx[0] = 0xaa;
                    tx[1] = 0x03;//通道号
                    tx[2] = BytesOP.GetLowNibble(p.ofs1_8);
                    tx[3] = BytesOP.GetHighNibble(p.ofs0_8);
                    tx[4] = BytesOP.GetLowNibble(p.ofs0_8);
                    tx[5] = 0xed;
                    com.Write(tx, 0, 6);
                    p.ofs_16 = (ushort)(b[4]);
                    tx[0] = 0xaa;
                    tx[1] = 0x05;//通道号
                    tx[2] = BytesOP.GetLowNibble(p.ofs1_8);
                    tx[3] = BytesOP.GetHighNibble(p.ofs0_8);
                    tx[4] = BytesOP.GetLowNibble(p.ofs0_8);
                    tx[5] = 0xed;
                    com.Write(tx, 0, 6);
                }
                else if (x == 2)//停止前后
                {
                    p.ofs_16 = (ushort)(b[3]);
                    tx[0] = 0xaa;
                    tx[1] = 0x04;//通道号
                    tx[2] = BytesOP.GetLowNibble(p.ofs1_8);
                    tx[3] = BytesOP.GetHighNibble(p.ofs0_8);
                    tx[4] = BytesOP.GetLowNibble(p.ofs0_8);
                    tx[5] = 0xed;
                    com.Write(tx, 0, 6);
                    p.ofs_16 = (ushort)(b[0]);
                    tx[0] = 0xaa;
                    tx[1] = 0x01;//通道号
                    tx[2] = BytesOP.GetLowNibble(p.ofs1_8);
                    tx[3] = BytesOP.GetHighNibble(p.ofs0_8);
                    tx[4] = BytesOP.GetLowNibble(p.ofs0_8);
                    tx[5] = 0xed;
                    com.Write(tx, 0, 6);
                }
                else if (x == 3)//停止上下
                {

                }
                else if (x >= 10)
                {
                    p.ofs_16 = (ushort)(b[x - 10]);
                    tx[0] = 0xaa;
                    tx[1] = (byte)(x-10+1);//通道号
                    tx[2] = BytesOP.GetLowNibble(p.ofs1_8);
                    tx[3] = BytesOP.GetHighNibble(p.ofs0_8);
                    tx[4] = BytesOP.GetLowNibble(p.ofs0_8);
                    tx[5] = 0xed;
                    com.Write(tx, 0, 6);
                }
                com.Close(); 
            }
        }
        bool OpenPort()
        {
            com = new SerialPort(Properties.Settings.Default.推进器通道, 9600);
            try
            {
                com.Open();
                return true;
            }
            catch (System.Exception ex)
            {
                MessageBox.Show("推进器通道串口无法打开:" + ex.Message);
                return false;
            }
        }
    }
}
